#include <stdio.h>
#include <string.h>
#include <openssl/cmac.h>
#include <openssl/aes.h>
#include <sys/time.h>

#define CLOCK_REALTIME 1

#define TM_START 1
double app_tminterval(int stop, int usertime)
{
    double ret = 0;
# ifdef CLOCK_REALTIME
    static struct timespec tmstart;
    struct timespec now;
# else
    static unsigned long tmstart;
    unsigned long now;
# endif
    static int warning = 1;

    if (usertime && warning) {
        printf("To get meaningful results, run "
                   "this program on idle system.\n");
        warning = 0;
    }
# ifdef CLOCK_REALTIME
    clock_gettime(CLOCK_REALTIME, &now);
    if (stop == TM_START)
        tmstart = now;
    else
        ret = ((now.tv_sec + now.tv_nsec * 1e-9)
               - (tmstart.tv_sec + tmstart.tv_nsec * 1e-9));
# else
    now = tickGet();
    if (stop == TM_START)
        tmstart = now;
    else
        ret = (now - tmstart) / (double)sysClkRateGet();
# endif
    return ret;
}

struct CMAC_CTX_st {
    /* Cipher context to use */
    EVP_CIPHER_CTX *cctx;
    unsigned char k1[EVP_MAX_BLOCK_LENGTH];
    // unsigned char k2[EVP_MAX_BLOCK_LENGTH];
    /* Temporary block */
    unsigned char tbl[EVP_MAX_BLOCK_LENGTH];
    /* Last (possibly partial) block */
    // unsigned char last_block[EVP_MAX_BLOCK_LENGTH];
    /* Number of bytes in last block: -1 means context not initialised */
    int nlast_block;
};


static void make_kn(unsigned char *k1, const unsigned char *l, int bl)
{
    int i;
    unsigned char c = l[0], carry = c >> 7, cnext;

    /* Shift block to left, including carry */
    for (i = 0; i < bl - 1; i++, c = cnext)
        k1[i] = (c << 1) | ((cnext = l[i + 1]) >> 7);

    /* If MSB set fixup with R */
    k1[i] = (c << 1) ^ ((0 - carry) & (bl == 16 ? 0x87 : 0x1b));
}


int MY_CMAC_Init(CMAC_CTX *ctx, const void *key, size_t keylen,
                 const EVP_CIPHER *cipher)
{
    static const unsigned char zero_iv[EVP_MAX_BLOCK_LENGTH] = { 0 };

    /* Initialise context */
    if (cipher != NULL) {
        /* Ensure we can't use this ctx until we also have a key */
        ctx->nlast_block = -1;
        if (!EVP_EncryptInit_ex(ctx->cctx, cipher, NULL, NULL, NULL))
            return 0;
    }
    /* Non-NULL key means initialisation complete */
    if (key != NULL) {
        /* If anything fails then ensure we can't use this ctx */
        ctx->nlast_block = -1;
        if (!EVP_EncryptInit_ex(ctx->cctx, NULL, NULL, key, zero_iv))
            return 0;
        // if (EVP_Cipher(ctx->cctx, ctx->tbl, zero_iv, 16) <= 0)
        //     return 0;
        unsigned char tbl[16] = {
            0xe5, 0x68, 0xf6, 0x81, 0x94, 0xCf, 0x76, 0xd6,
            0x17, 0x4d, 0x4c, 0xc0, 0x43, 0x10, 0xa8, 0x54
        };

        make_kn(ctx->k1, tbl, 16);
        // make_kn(ctx->k1, ctx->tbl, 16);
        // OPENSSL_cleanse(ctx->tbl, 16);
        /* Reset context again ready for first data block */
        if (!EVP_EncryptInit_ex(ctx->cctx, NULL, NULL, NULL, zero_iv))
            return 0;
        // /* Zero tbl so resume works */
        // memset(ctx->tbl, 0, 16);
        ctx->nlast_block = 16;
    }
    return 1;
}

// int MY_CMAC_Update(CMAC_CTX *ctx, const void *in)
// {
//     memcpy(ctx->last_block, in, 16);
//     return 1;
// }

int MY_CMAC_Final(CMAC_CTX *ctx, unsigned char *message, unsigned char *out, size_t *poutlen)
{
    *poutlen = (size_t)16;
    for (int i = 0; i < 16; i++)
        out[i] = message[i] ^ ctx->k1[i];
    if (EVP_Cipher(ctx->cctx, out, out, 16) <= 0) {
        OPENSSL_cleanse(out, 16);
        return 0;
    }
    return 1;
}
void cal_cmac(CMAC_CTX *ctx,unsigned char *key,unsigned char *message) {

    uint8_t cmac[EVP_MAX_BLOCK_LENGTH];
    size_t cmac_len;
    if (!CMAC_Init(ctx, key, 32, EVP_aes_256_cbc(), NULL) || 
        !CMAC_Update(ctx, message, 16) ||
        !CMAC_Final(ctx, cmac, &cmac_len))
        printf("cmac error!");

    // printf("CMAC: ");
    // for (size_t i = 0; i < cmac_len; i++) {
    //     printf("%02x", cmac[i]);
    // }
    // printf("\n");
}


void cal_cmac2(CMAC_CTX *ctx,unsigned char *key,unsigned char *message) {

    uint8_t cmac[EVP_MAX_BLOCK_LENGTH];
    size_t cmac_len;
    if (!MY_CMAC_Init(ctx, key, 32, EVP_aes_256_cbc()) || 
        // !MY_CMAC_Update(ctx, message) ||
        !MY_CMAC_Final(ctx, message, cmac, &cmac_len))
        printf("cmac error!");

    // printf("CMAC: ");
    // for (size_t i = 0; i < cmac_len; i++) {
    //     printf("%02x", cmac[i]);
    // }
    // printf("\n");
}
int main() {
    unsigned char key[32] = {
        0x60, 0x3d, 0xeb, 0x10, 0x15, 0xca, 0x71, 0xbe,
        0x2b, 0x73, 0xae, 0xf0, 0x85, 0x7d, 0x77, 0x81,
        0x1f, 0x35, 0x2c, 0x07, 0x3b, 0x61, 0x08, 0xd7,
        0x2d, 0x98, 0x10, 0xa3, 0x09, 0x14, 0xdf, 0xf4
    };
    unsigned char ikm[16] = {
        0x60, 0x3d, 0xeb, 0x10, 0x15, 0xca, 0x71, 0xbe,
        0x2b, 0x73, 0xae, 0xf0, 0x85, 0x7d, 0x77, 0x81
    };
    double cost_time = 0.0;
    CMAC_CTX *ctx = CMAC_CTX_new();
    app_tminterval(TM_START, 0);
    cal_cmac(ctx, key, ikm);
    cost_time = app_tminterval(0, 0);
    printf("cost_time:%.2fns\n",cost_time * 1000000000);
    CMAC_CTX_free(ctx);

    ctx = CMAC_CTX_new();
    app_tminterval(TM_START, 0);
    cal_cmac(ctx, key, ikm);
    cost_time = app_tminterval(0, 0);
    printf("cost_time:%.2fns\n",cost_time * 1000000000);
    CMAC_CTX_free(ctx);

    ctx = CMAC_CTX_new();
    app_tminterval(TM_START, 0);
    cal_cmac2(ctx, key, ikm);
    cost_time = app_tminterval(0, 0);
    printf("cost_time:%.2fns\n",cost_time * 1000000000);
    CMAC_CTX_free(ctx);
    return 0;
}

